"""
brief :Editor cjh
"""
import pyrealsense2 as rs
import cv2
import numpy as np
import random
import logging

logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(message)s')
logger = logging.getLogger("Realsense_init")


class realsense(object):
    def __init__(self, device_id=None):
        # inter setting
        self.align = None
        self.depth_intrin = None
        self.color_intrin = None
        self.aligned_frames = None
        self.pipeline = rs.pipeline(rs.context())
        self.config = rs.config()
        self.device_id = device_id
        if self.device_id is None:  # 通过设备的序列号打开深度相机，如果设备相机不存在，则返回相机串口号
            self.check_device_id()
        elif self.device_id is not None:  # 通过设备的序列号打开深度相机，如果当设备相机存在
            self.config.enable_device(self.device_id)
        self.width, self.height = int(), int()

    @staticmethod
    def check_device_id():
        # 相机选择初始化,获得相机的序列号
        ctx = rs.context()
        serial_rs = ctx.devices[0].get_info(rs.camera_info.serial_number)
        logger.info(f"Realsense camera serial Information :{serial_rs}")

    def cam_init(self, ppi=640, enable_auto_expose=True, enable_auto_white_balance=True, exposure=10000):
        if ppi == 1080:
            self.config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
            self.config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
            self.width, self.height = 1280, 720
        else:
            self.config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 60)
            self.config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 60)
            self.width, self.height = 848, 480
        try:
            self.pipeline.stop()  # 流程开始,先确认是否关闭
            logger.info(f"is opened before")
        except Exception:
            pass

        self.pipeline.start(self.config)  # 流程开始
        # 确认是否开启自动曝光或开启自动白平衡
        sensor = self.pipeline.get_active_profile().get_device().query_sensors()[1]
        sensor.set_option(rs.option.enable_auto_white_balance, enable_auto_white_balance)
        sensor.set_option(rs.option.enable_auto_exposure, enable_auto_expose)
        if not enable_auto_expose:
            sensor.set_option(rs.option.exposure, exposure)
        align_to = rs.stream.color  # 与color流对齐
        self.align = rs.align(align_to)
        logger.info(f"Realsense camera successfully init :{self.width}x{self.height}")

    def cam_run(self):
        if self.align is not None:
            frames = self.pipeline.wait_for_frames()  # 等待获取图像帧
            self.aligned_frames = self.align.process(frames)  # 获取对齐帧
            aligned_depth_frame = self.aligned_frames.get_depth_frame()  # 获取对齐帧中的depth帧
            aligned_color_frame = self.aligned_frames.get_color_frame()  # 获取对齐帧中的color帧
            ############### 相机参数的获取 #######################
            # intr = color_frame.profile.as_video_stream_profile().intrinsics  # 获取相机内参
            self.depth_intrin = aligned_depth_frame.profile.as_video_stream_profile().intrinsics  # 获取深度参数（像素坐标系转相机坐标系会用到）
            # self.color_intrin = aligned_color_frame.profile.as_video_stream_profile().intrinsics
            depth_image = np.asanyarray(aligned_depth_frame.get_data())  # 深度图（默认16位），由于对齐RGB图因此变为8位
            color_image = np.asanyarray(aligned_color_frame.get_data())  # RGB图
            depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03),
                                               cv2.COLORMAP_JET)  # 转换为色彩深度图
            # 以下返回值包括八位RGB图像，八位深度信息映射RGB图像
            return color_image.astype(np.uint8), depth_colormap.astype(np.uint8)
        else:
            self.cam_init()
            return self.cam_run()

    # deal with depth frame data
    def depth_to_data(self, mid_pos, min_val=20):
        distance_list = []
        randnum = 80
        for i in range(randnum):
            bias = random.randint(-min_val // 20, min_val // 20)  # 最小区域噪声，用于计算均值深度
            pos1, pos2 = int(mid_pos[0] + bias), int(mid_pos[1] + bias)
            dist = self.aligned_frames.get_depth_frame().get_distance(pos1 if pos1 <= self.width else pos1 < self.width,
                                                                      pos2 if pos2 <= self.height else pos1 <= self.height)
            if dist:
                distance_list.append(dist)
                if np.var(distance_list) > 3:  # 添加方差，以至于不会造成值突变
                    distance_list.pop()
        distance_list = np.array(distance_list)
        distance_list = np.sort(distance_list)[randnum // 2 - randnum // 4:randnum // 2 + randnum // 4]
        if len(distance_list) > 0:
            mean_distance = np.nanmean(distance_list)
        else:
            mean_distance = 0
        camera_coordinate = rs.rs2_deproject_pixel_to_point(self.depth_intrin, mid_pos,
                                                            mean_distance)  # 相机内参和深度逆矩阵运算，可得出基于相机坐标系下的，target三维信息
        return camera_coordinate


if __name__ == "__main__":
    real = realsense(device_id=None)
    real.cam_init(enable_auto_expose=True)
    # usage introduction
    while True:
        # 以下返回值包括八位RGB图像，八位深度信息映射RGB图像，对齐的深度帧信息
        color_image, depth_colormap = real.cam_run()
        images_real = np.hstack((color_image, depth_colormap))
        mid_pos = (int(color_image.shape[1] / 2) + 50, int(color_image.shape[0] / 2))
        x, y, z = real.depth_to_data(mid_pos, 20)
        print(z)
        cv2.imshow("real", images_real)
        if cv2.waitKey(1) == 27:
            break
